/* USER CODE BEGIN Header */
/**
  **************************************************************************************************
  * @file           : can_motor_task.c
  * @brief          : can电机输出控制freertos任务源文件
  **************************************************************************************************
  * @attention
  *	请在 *外部变量&头文件链接区* 内链接电机用户层（can_Motor_user.c）内存储电机参数的结构体
  **************************************************************************************************
  * @needToDo
  * 在main.c中请在osKernelInitialize内核初始化函数之后调用can_motor_task_init	
  **************************************************************************************************
  */
/* USER CODE END Header */
#include "can_motor_task.h"
#include "FreeRTOS.h"
#include "task.h"
#include "cmsis_os.h"
#include "bsp_can.h"
	

/*	外部变量&头文件链接区 BEGIN	*/
	#include "pid.h"
	#include "Can_Motor_drv.h"
	#include "Can_Motor_user.h"
	//链接电机用户层中存储电机参数的结构体
	extern Motor_measure_t* pmmt_can1_fifo0[can1_fifo0_motor_cnt];//can1
	extern Motor_measure_t* pmmt_can2_fifo0[can2_fifo0_motor_cnt];//can2
	//链接电机用户层中电机pid参数结构体
	extern pid_t* p_spid_can1_fifo0[can1_fifo0_motor_cnt];	//can1
	extern pid_t* p_apid_can1_fifo0[can1_fifo0_motor_cnt];
	extern pid_t* p_spid_can2_fifo0[can2_fifo0_motor_cnt];	//can2
	extern pid_t* p_apid_can2_fifo0[can2_fifo0_motor_cnt];
	
	
	extern motor_angle m2006_angle;
	
/*	外部变量&头文件链接区 END	*/

#define UPDATE_CAN1 1
#define UPDATE_CAN2 2
can_msg_queue_t queue_can1_fifo0[can1_fifo0_motor_cnt];	//存放can1 FIFO0的can报文队列数组
can_msg_queue_t queue_can2_fifo0[can2_fifo0_motor_cnt];	//存放can2 FIFO0的can报文队列数组

/**
 * @brief 电机角度环pid更新累计转过角度(angle_total)的函数
 */
static void get_total_angle(Motor_measure_t* pmmt)
{	
    // 计算原始角度差
    float delta =  pmmt->angle_now - pmmt->angle_last;
    // 处理360°边界情况（修正角度差方向）
    if (delta > 180.0f) {
        delta -= 360.0f;  // 正转超过180°视为反转
    } else if (delta < -180.0f) {
        delta += 360.0f;  // 反转超过180°视为正转
    }
    // 累加总角度
    pmmt->angle_total += delta;
}

//RTOS任务参数
void Can1ContrlTask(void *argument);
void Can2ContrlTask(void *argument);
static void Can1Angle(void);
static void Can2Angle(void);
static void update_motor_measure(uint8_t where);
osThreadId_t	Can1ContrlHandle;		//can1电机控制任务
osThreadId_t	Can2ContrlHandle;		//can2电机控制任务

const osThreadAttr_t Can1ContrlTask_attributes = {
  .name = "Can1ContrlTask",
  .stack_size = 128 * 4,
  .priority = (osPriority_t) osPriorityBelowNormal7,	//优先级belownormal17(比normal低一级)
};
const osThreadAttr_t Can2ContrlTask_attributes = {
  .name = "Can2ContrlTask",
  .stack_size = 128 * 4,
  .priority = (osPriority_t) osPriorityBelowNormal7,	//优先级belownormal17(比normal低一级)
};

/***************************************************************************/
/*********************************函数定义区********************************/
/***************************************************************************/

void can_motor_task_init(void)
{
	//创建任务
	Can1ContrlHandle = osThreadNew(Can1ContrlTask, NULL, &Can1ContrlTask_attributes);
	Can2ContrlHandle = osThreadNew(Can2ContrlTask, NULL, &Can2ContrlTask_attributes);
	
	//创建队列
	for(int i = 0;i < can1_fifo0_motor_cnt;++i)
	{
		queue_can1_fifo0[i].que = osMessageQueueNew(1,sizeof(can_recive_msg_t),NULL);
	}
	for(int i = 0;i < can2_fifo0_motor_cnt;++i)
	{
		queue_can2_fifo0[i].que = osMessageQueueNew(1,sizeof(can_recive_msg_t),NULL);;
	}
	{//设置队列的can ID
		queue_can1_fifo0[0].stdID = 0x201;
		queue_can1_fifo0[1].stdID = 0x202;
		queue_can1_fifo0[2].stdID = 0x203;
		queue_can1_fifo0[3].stdID = 0x204;
		queue_can1_fifo0[4].stdID = 0x205;
		
		queue_can2_fifo0[0].stdID = 0x201;
		queue_can2_fifo0[1].stdID = 0x202;
		queue_can2_fifo0[2].stdID = 0x203;
		queue_can2_fifo0[3].stdID = 0x204;
		queue_can2_fifo0[4].stdID = 0x205;
	}
}


/**
 * @brief can1电机pid调速任务
 */

void Can1ContrlTask(void *argument)
{
	static int16_t can1_out[can1_fifo0_motor_cnt] = {0};
	//主循环(速度环pid的计算)
	for(;;)
	{
		update_motor_measure(UPDATE_CAN1);	//更新数据
		Can1Angle();		  //角度环计算
		
		//速度环计算及输出
		for(int ii = 0;ii < can1_fifo0_motor_cnt;++ii)
		{
			//是否让电机动
			if(pmmt_can1_fifo0[ii] -> state  ==  RUN)
			{
				//判断是否稳定在指定的位置
				if(PID_STABLE_CHECK(p_spid_can1_fifo0[ii]) == 1)	//处于稳态
				{
					////*****************************
				}
				
				//由目标转速计算出应输出的电流
				if(ii < 4)	//底盘
				{
					can1_out[ii] = chassis_pid_calc(p_spid_can1_fifo0[ii], 
												pmmt_can1_fifo0[ii]-> speed_rpm_now, 
												pmmt_can1_fifo0[ii]-> target_speed);
				}
				else		
				{
					can1_out[ii] = pid_calc(p_spid_can1_fifo0[ii], 
												pmmt_can1_fifo0[ii]-> speed_rpm_now, 
												pmmt_can1_fifo0[ii]-> target_speed);
				}
			}
			else
			{
				can1_out[ii] = 0;
			}
		}
		
		//0x200
		Can_Motor_cmd(&hcan1,0x200,can1_out[0],can1_out[1],can1_out[2],can1_out[3]);
		//0x1ff
		Can_Motor_cmd(&hcan1,0x1ff,can1_out[4],0,0,0);
		osDelay(1);
	}
}


/**
 * @brief can1电机角度环pid,结合速度环实现串级pid
 */
static void Can1Angle(void)
{	
		//角度环pid的计算
		for(int ii = 0;ii < can1_fifo0_motor_cnt;++ii)
		{
			if(pmmt_can1_fifo0[ii] -> angle_mode  !=  ANGLE_NO)
			{
				//判断是否稳定在指定的位置
				if(PID_STABLE_CHECK(p_apid_can1_fifo0[ii]) == 1)	//处于稳态
				{
					////*****************************
				}
				
				get_total_angle(pmmt_can1_fifo0[ii]);	//更新总角度
				pmmt_can1_fifo0[ii] -> target_speed = pid_calc(p_apid_can1_fifo0[ii],
						pmmt_can1_fifo0[ii]-> angle_total, 
						pmmt_can1_fifo0[ii]-> target_angle);
			}
			else if(pmmt_can1_fifo0[ii] -> angle_mode  ==  ANGLE_NO){continue;}
		}
}

/**
 * @brief can2电机pid调速任务
 */

void Can2ContrlTask(void *argument)
{
	static int16_t can2_out[can2_fifo0_motor_cnt] = {0};
	for(;;)
	{
		update_motor_measure(UPDATE_CAN2);	//更新数据
		Can2Angle();				//先算角度环
		
		//速度环计算以及输出
		for(int i = 0;i < can2_fifo0_motor_cnt;++i)
		{
			//是否让电机动
			if(pmmt_can2_fifo0[i] -> state  ==  RUN)
			{
				//判断是否稳定在指定的位置
				if(PID_STABLE_CHECK(p_spid_can2_fifo0[i]) == 1)	//处于稳态
				{
					////*****************************
				}
				
				//由目标转速计算出应输出的电流
				can2_out[i] = pid_calc(p_spid_can2_fifo0[i], 
											pmmt_can2_fifo0[i]-> speed_rpm_now, 
											pmmt_can2_fifo0[i]-> target_speed);
			}
			else
			{
				can2_out[i] = 0;
			}
		}
		
		//0x200
		Can_Motor_cmd(&hcan2,0x200,can2_out[0],can2_out[1],can2_out[2],can2_out[3]);
		//发射2006
		//0x1ff
		Can_Motor_cmd(&hcan2,0x1ff,can2_out[4],0,0,0);
		osDelay(1);
	}
}

/**
 * @brief can2电机角度环pid,结合速度环实现串级pid,UpLa9yerSpeedTask
 */
static void Can2Angle(void)
{
		for(int i = 0;i < can2_fifo0_motor_cnt;++i)
		{
			if(pmmt_can2_fifo0[i] -> angle_mode  !=  ANGLE_NO)
			{
				//判断是否稳定在指定的位置
				if(PID_STABLE_CHECK(p_apid_can2_fifo0[i]) == 1)	//处于稳态
				{
					////*****************************
				}
				
				get_total_angle(pmmt_can2_fifo0[i]);	//更新总角度
				pmmt_can2_fifo0[i] -> target_speed = pid_calc(p_apid_can2_fifo0[i],
						pmmt_can2_fifo0[i]-> angle_total, 
						pmmt_can2_fifo0[i]-> target_angle);
			}
			else if(pmmt_can2_fifo0[i] -> angle_mode  ==  ANGLE_NO){continue;}
		}
		//累计2006转过的总角度
        {
            float delta =  m_0x205_can2.angle_now - m_0x205_can2.angle_last;
            // 处理360°边界情况（修正角度差方向）
            if (delta > 180.0f) {
                delta -= 360.0f;  // 正转超过180°视为反转
            } else if (delta < -180.0f) {
                delta += 360.0f;  // 反转超过180°视为正转
            }
            // 累加总角度
            m2006_angle.total_angle += delta;
        }
}



/**
 * @brief 更新电机参数
 * @param where 	更新的是can1的还是can2的 @args UPDATE_CAN1(can1) , UPDATE_CAN2(can2)
 */
static void update_motor_measure(uint8_t where)
{
	//更新can1电机反馈数据
	if(where == 1)
	{
		for (int i = 0;i < can1_fifo0_motor_cnt;++i)
		{
			can_recive_msg_t temp_msg = {0};
			//从队列中取数据,不等待
			osStatus_t qstatus = osMessageQueueGet(queue_can1_fifo0[i].que,&temp_msg,NULL,0);
			if(qstatus == osOK)
			{
				//更新转速
				pmmt_can1_fifo0[i]->speed_rpm_last = pmmt_can1_fifo0[i]->speed_rpm_now;
				int16_t spnow = (int16_t)temp_msg.data[2] << 8 | temp_msg.data[3];
				pmmt_can1_fifo0[i]->speed_rpm_now = spnow;
				//更新角度
				pmmt_can1_fifo0[i]->angle_last = pmmt_can1_fifo0[i]->angle_now;
				int16_t agnow = (int16_t)temp_msg.data[0] << 8 | temp_msg.data[1];
				pmmt_can1_fifo0[i]->angle_now = (float)agnow * 360.0f / 8192.0f;	
			}
		}
	}
	//更新can2电机反馈数据
	else if(where == 2)
	{
		for (int i = 0;i < can2_fifo0_motor_cnt;++i)
		{
			can_recive_msg_t temp_msg = {0};
			osStatus_t qstatus = osMessageQueueGet(queue_can2_fifo0[i].que,&temp_msg,NULL,0);
			if(qstatus == osOK)
			{
				pmmt_can2_fifo0[i]->speed_rpm_last = pmmt_can2_fifo0[i]->speed_rpm_now;
				int16_t spnow = (int16_t)temp_msg.data[2] << 8 | temp_msg.data[3];
				pmmt_can2_fifo0[i]->speed_rpm_now = spnow;
				//更新角度
				pmmt_can2_fifo0[i]->angle_last = pmmt_can2_fifo0[i]->angle_now;
				int16_t agnow = (int16_t)temp_msg.data[0] << 8 | temp_msg.data[1];
				pmmt_can2_fifo0[i]->angle_now = (float)agnow * 360.0f / 8192.0f;
			}
		}
	}
}
//




